/**
 * @file AStarPlanner.h
 * @brief A*算法路径规划器
 * @details
 * A*算法原理：
 * 1. 使用评估函数 f(n) = g(n) + h(n)
 * 2. g(n): 从起点到节点n的实际代价
 * 3. h(n): 从节点n到目标的估计代价 (启发式函数)
 * 4. 每次选择f(n)最小的节点进行扩展
 * 5. 保证找到最短路径，且比Dijkstra更高效
 *
 * 启发式函数选择：
 * - 欧几里得距离: 可对角移动
 * - 曼哈顿距离: 只能水平垂直移动
 */

#ifndef ASTAR_PLANNER_H
#define ASTAR_PLANNER_H

#include "core/PlannerBase.h"
#include <queue>
#include <unordered_map>
#include <cmath>

namespace PathPlanning
{
/**
 * @brief A*算法配置参数
 */
struct AStarConfig : public PlannerConfig
{
    bool        eightConnected    = true; // 使用8方向连接
    double      obstacleThreshold = 128.0;
    std::string heuristicType     = "euclidean"; // "euclidean" or "manhattan"
    double      weight            = 1.0;         // 启发式权重

    AStarConfig()
    {
        plannerName = "AStar";
    }
};

/**
 * @brief A*搜索节点
 */
struct AStarNode : public Node
{
    GridCell gridCell;

    AStarNode(const GridCell& cell, const Point& worldPos) : gridCell(cell)
    {
        position = worldPos;
    }
};

/**
 * @brief A*节点比较器
 */
struct AStarNodeCompare
{
    bool operator()(const std::shared_ptr<AStarNode>& a, const std::shared_ptr<AStarNode>& b) const
    {
        return a->fCost > b->fCost; // 最小堆
    }
};

class AStarPlanner : public PlannerBase
{
public:
    AStarPlanner(std::shared_ptr<Map> map);

    PlanningResult plan(const Point& start, const Point& goal) override;

private:
    /**
     * @brief 计算启发式函数值
     */
    double calculateHeuristic(const Point& from, const Point& to);

    /**
     * @brief 从网格坐标重建路径
     */
    std::vector<Pose>
    reconstructPath(const std::unordered_map<GridCell, std::shared_ptr<AStarNode>, GridCellHash>& cameFrom,
                    const std::shared_ptr<AStarNode>&                                             endNode);
};

} // namespace PathPlanning

#endif